/**
  ******************************************************************************
  * @file           : BMI160.c
  * @brief          : Driving program for BMI160
  ******************************************************************************
  */

#include "BMI160.h"
#include <stdio.h>
#include <stdint.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <wiringPiI2C.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h> 
#include <string.h>
int fdI2C;
BMI160_Data GYRO_OFFSET;

void initBMI160()
{

    wiringPiSetup () ;
    fdI2C = wiringPiI2CSetup (BMI160_ADDR);  
    if (fdI2C >= 0)
    { // fd 为负数，说明IIC连接失败
        wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_SOFTRESET);
	delay(100);
	wiringPiI2CWriteReg8(fdI2C,BMI160_FOC_CONF,0x40);
	wiringPiI2CWriteReg8(fdI2C,BMI160_OFFSET_EN,0x80+0x40);
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_START_FOC);
	
	delay(200);
	/* range & rate setting */
        wiringPiI2CWriteReg8(fdI2C,BMI160_ACC_CONF,0x08);
	wiringPiI2CWriteReg8(fdI2C,BMI160_ACC_RANGE,0x03);
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_CONF,0x08); 
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_RANGE,0x00);
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_RANGE,0x00);
	/*FIFO setting*/
	/*wiringPiI2CWriteReg8(fd,BMI160_FIFO_DOWNS,0xff);
	wiringPiI2CWriteReg8(fd,BMI160_FIFO_CONFIG1,0x0f);
	wiringPiI2CWriteReg8(fd,BMI160_FIFO_CONFIG2,0xF0);*/
	/*IMU Begin*/
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_ACCEL_NORMAL);
	delay(10);
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_GYRO_NORMAL);
	delay(100);
	BMI160_Data gb,g;
	g.x=0;
	g.y=0;
	g.z=0;
	for(int i=0;i<100;i++)
	{
	  gb=BMI160_GetGyro();
	  g.x+=gb.x;
	  g.y+=gb.y;
	  g.z+=gb.z;
	}
	GYRO_OFFSET.x=g.x/100;
	GYRO_OFFSET.y=g.y/100;
	GYRO_OFFSET.z=g.z/100;
	printf("offset:%d,%d,%d",GYRO_OFFSET.x,GYRO_OFFSET.y,GYRO_OFFSET.z);
        printf("[ok]BMI160 Init\n");
    }
    else {
        printf("[Failed]BMI160 Init\n");
    }
}

BMI160_Data BMI160_GetAccel(void)
{
	static BMI160_Data temp;
	//wiringPiI2CWriteReg8(fd,BMI160_CMD,BMI160CMD_ACCEL_NORMAL);
	//delay(10);
	temp.x=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_XOUTH_REG)<<8);
	temp.x|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_XOUTL_REG);
	temp.y=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_YOUTH_REG)<<8);
	temp.y|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_YOUTL_REG);
	temp.z=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_ZOUTH_REG)<<8);
	temp.z|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_ZOUTL_REG);
	if(temp.x>0x7fff)
            temp.x = -(0xffff-temp.x);
	if(temp.y>0x7fff)
            temp.y = -(0xffff-temp.y);
	if(temp.z>0x7fff)
            temp.z = -(0xffff-temp.z);
	temp.x-=GYRO_OFFSET.x;
	temp.y-=GYRO_OFFSET.y;
	temp.z-=GYRO_OFFSET.z;
	return temp;
}

BMI160_Data BMI160_GetGyro(void)
{
	static BMI160_Data temp;
	//wiringPiI2CWriteReg8(fd,BMI160_CMD,BMI160CMD_GYRO_NORMAL);
	//delay(50);		
	temp.x=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_XOUTH_REG)<<8);
	temp.x|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_XOUTL_REG);
	temp.y=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_YOUTH_REG)<<8);
	temp.y|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_YOUTL_REG);
	temp.z=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_ZOUTH_REG)<<8);
	temp.z|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_ZOUTL_REG);
	if(temp.x>0x7fff)
            temp.x = -(0xffff-temp.x);
	if(temp.y>0x7fff)
            temp.y = -(0xffff-temp.y);
	if(temp.z>0x7fff)
            temp.z = -(0xffff-temp.z);
	return temp;
}
